#include "Pose2Matrix.h"

MatrixXd pose2matrix(4, 4);
void Pose2Matrix(double pose[])
{
	double Matrix[4][4];
	double s[3] = { 0, };
	double c[3] = { 0, };
	double s2[3] = { 0, };
	double c2[3] = { 0, };
	c[0] = cos(pose[3]);
	c[1] = cos(pose[4]);
	c[2] = cos(pose[5]);
	s[0] = sin(pose[3]);
	s[1] = sin(pose[4]);
	s[2] = sin(pose[5]);


	Matrix[0][0] = c[2] * c[1];
	Matrix[0][1] = c[2] * s[1] * s[0] - s[2] * c[0];
	Matrix[0][2] = c[2] * s[1] * c[0] + s[2] * s[0];
	Matrix[0][3] = pose[0];

	Matrix[1][0] = s[2] * c[1];
	Matrix[1][1] = s[2] * s[1] * s[0] + c[2] * c[0];
	Matrix[1][2] = s[2] * s[1] * c[0] - c[2] * s[0];
	Matrix[1][3] = pose[1];

	Matrix[2][0] = -s[1];
	Matrix[2][1] = c[1] * s[0];
	Matrix[2][2] = c[1] * c[0];
	Matrix[2][3] = pose[2];

	Matrix[3][0] = 0;
	Matrix[3][1] = 0;
	Matrix[3][2] = 0;
	Matrix[3][3] = 1;

	pose2matrix << Matrix[0][0], Matrix[0][1], Matrix[0][2], Matrix[0][3],
		Matrix[1][0], Matrix[1][1], Matrix[1][2], Matrix[1][3],
		Matrix[2][0], Matrix[2][1], Matrix[2][2], Matrix[2][3],
		Matrix[3][0], Matrix[3][1], Matrix[3][2], Matrix[3][3];
	cout << "pose2matrix:" << pose2matrix << endl;
}